#!/usr/bin/env python
import roslib; roslib.load_manifest('daido_driver')
import rospy
from geometry_msgs.msg import Point
from geometry_msgs.msg import Twist

class DaidoNav:

    def __init__(self):
        self.twist_pub_ = rospy.Publisher('/base_controller/command', Twist)
        self.last_time_found_face_ = False

    def face_pos_cb(self, face_location_msg):
        #rospy.loginfo( rospy.get_name()+ " I heard %f", face_location_msg.x)
    
        # Initialize message
        val = Twist()
        val.linear.x = 0
        val.linear.y = 0
        val.linear.z = 0
        val.angular.x = 0
        val.angular.y = 0
        val.angular.z = 0

        # Unconditionally publish if we found a face last time
        need_publish = False or self.last_time_found_face_

        # Publish new message to drive robot one way or the other
        if face_location_msg.x >= 0 and face_location_msg.y >=0:
            self.last_time_found_face_ = True
            need_publish = True

            # Thresholds
            r_thresh = 0.65
            l_thresh = 0.35
            #print face_location_msg.x
            # Go straight
            if face_location_msg.x < r_thresh and \
                    face_location_msg.x > l_thresh: 
                val.linear.x = 0.6
            elif face_location_msg.x >= r_thresh:
                #  Turn right
                val.angular.z = \
                    0.5 + (0.5*((face_location_msg.x-r_thresh)/(1.0-r_thresh)))
                val.angular.z *= -1.0
            elif face_location_msg.x <= l_thresh:
                # Turn left
                val.angular.z = \
                    0.5 + (0.5*((l_thresh-face_location_msg.x)/(l_thresh)))
        else:
            # Did not find face
            self.last_time_found_face_ = False

        if need_publish == True:
            self.twist_pub_.publish( val )

    def listener(self):
        # rospy.init_node('daido_nav', anonymous=True)
        rospy.init_node('daido_nav')
        rospy.Subscriber("/daido_face_locate/face_position", Point, self.face_pos_cb)
        rospy.spin()

if __name__ == '__main__':
    dn = DaidoNav()
    dn.listener()
